﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;
using Microsoft.Kinect;
using System.Text;
using System.Threading;
using System.IO;
using System.Windows.Input;
using System.Windows.Controls.Primitives;



namespace Microsoft.Samples.Kinect.WpfViewers {

    /// <summary>
    /// Interaction logic for KinectDiagnosticViewer.xaml
    /// </summary>
    public partial class KinectDiagnosticViewer : System.Windows.Controls.UserControl {


        KinectPedal Pedal = new KinectPedal();
        KinectRun Run = new KinectRun();

        bool isBiking = true;

        bool isFront = true;

        Button currentPreset = null;

        [DllImport("user32.dll", EntryPoint = "keybd_event", CharSet = CharSet.Auto, ExactSpelling = true)]
        public static extern void Keybd_event(byte vk, byte scan, int flags, int extrainfo);


        string LogFile = "Steps" + DateTime.Now.Month + "_" + DateTime.Now.Day + "_" + DateTime.Now.Year + ".txt";


        #region Public API
        public KinectDiagnosticViewer() {
            InitializeComponent();
            currentPreset = Preset1;
        }

        public void ReInitRuntime() {
            // Will call Uninitialize followed by Initialize.
            this.Kinect = this.Kinect;
        }

        public KinectSensor Kinect {
            get { return _Kinect; }
            set {
                //Clean up existing runtime if we are being set to null, or a new Runtime.
                if (_Kinect != null) {
                    kinectColorViewer.Kinect = null;
                    kinectDepthViewer.Kinect = null;
                    _Kinect.SkeletonFrameReady -= new EventHandler<SkeletonFrameReadyEventArgs>(SkeletonFrameReady);
                    _Kinect.Stop();
                }

                _Kinect = value;

                if (_Kinect != null) {

                    kinectColorViewer.Kinect = _Kinect;

                    _Kinect.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(SkeletonFrameReady);
                    UpdateUi();
                }
            }
        }

        //Status and InstanceIndex can change. Those properties in the Runtime should be made to support INotifyPropertyChange.
        public void UpdateUi() {
            kinectIndex.Text = _Kinect.DeviceConnectionId.ToString();
            kinectName.Text = _Kinect.DeviceConnectionId;
            kinectStatus.Text = _Kinect.Status.ToString();
        }
        #endregion Public API

        #region Skeleton Processing
        private void SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e) {
            SkeletonFrame skeletonFrame = e.OpenSkeletonFrame();

            //KinectSDK TODO: this shouldn't be needed, but if power is removed from the Kinect, you may still get an event here, but skeletonFrame will be null.
            if (skeletonFrame == null || _Kinect==null) {
                return;
            }

            int iSkeleton = 0;
            Brush[] brushes = new Brush[6];
            brushes[0] = new SolidColorBrush(Color.FromRgb(255, 0, 0));
            brushes[1] = new SolidColorBrush(Color.FromRgb(0, 255, 0));
            brushes[2] = new SolidColorBrush(Color.FromRgb(64, 255, 255));
            brushes[3] = new SolidColorBrush(Color.FromRgb(255, 255, 64));
            brushes[4] = new SolidColorBrush(Color.FromRgb(255, 64, 255));
            brushes[5] = new SolidColorBrush(Color.FromRgb(128, 128, 255));


            skeletonCanvas.Children.Clear();
            Skeleton[] skels = new Skeleton[skeletonFrame.SkeletonArrayLength];
            skeletonFrame.CopySkeletonDataTo(skels);
            //foreach (SkeletonData data in skeletonFrame.Skeletons) {
            foreach (Skeleton data in skels) {
                if (SkeletonTrackingState.Tracked == data.TrackingState) {
                    // Draw bones
                    Brush brush = brushes[iSkeleton % brushes.Length];
                    skeletonCanvas.Children.Add(getBodySegment(data.Joints, brush, JointType.HipCenter, JointType.Spine, JointType.ShoulderCenter, JointType.Head));
                    skeletonCanvas.Children.Add(getBodySegment(data.Joints, brush, JointType.ShoulderCenter, JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, JointType.HandLeft));
                    skeletonCanvas.Children.Add(getBodySegment(data.Joints, brush, JointType.ShoulderCenter, JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, JointType.HandRight));
                    skeletonCanvas.Children.Add(getBodySegment(data.Joints, brush, JointType.HipCenter, JointType.HipLeft, JointType.KneeLeft, JointType.AnkleLeft, JointType.FootLeft));
                    skeletonCanvas.Children.Add(getBodySegment(data.Joints, brush, JointType.HipCenter, JointType.HipRight, JointType.KneeRight, JointType.AnkleRight, JointType.FootRight));

                    // Draw joints
                    foreach (Joint joint in data.Joints) {
                        Point jointPos = getDisplayPosition(joint);
                        Line jointLine = new Line();
                        jointLine.X1 = jointPos.X - 3;
                        jointLine.X2 = jointLine.X1 + 6;
                        jointLine.Y1 = jointLine.Y2 = jointPos.Y;
                        jointLine.Stroke = jointColors[joint.JointType];
                        jointLine.StrokeThickness = 6;
                        skeletonCanvas.Children.Add(jointLine);
                    }

                    float center = data.Joints[JointType.HipCenter].Position.Z;
                    float lean = data.Joints[JointType.ShoulderCenter].Position.Z;
                    tb_A.Text = (center - lean).ToString();
                    if (isBiking)
                    Pedal.UpdateSkeleton(data);
                    else
                    Run.UpdateSkeleton(data);
                    break;

                    
                }
                iSkeleton++;
            } // for each skeleton

            if (isBiking) {
                Pedal.CheckMovement();
                Pedal.MovePlayer();
                if (Pedal.HasGas) {
                    tb_Gas.Text = "Gas";
                } else {
                    tb_Gas.Text = "No Gas";
                }
                if (Pedal.HasBoost) {
                    tb_Boost.Text = "Boost";
                } else {
                    tb_Boost.Text = "";
                }
                tb_Lean.Text = Pedal.Lean;
                tb_Wheelie.Text = Pedal.Wheelie;
            } else {
                Run.CheckMovement();
                Run.MovePlayer();
                if (Run.HasGas) {
                    tb_Gas.Text = "Run";
                } else {
                    tb_Gas.Text = "No Run";
                }
                if (Run.HasBoost) {
                    tb_Boost.Text = "Boost";
                } else {
                    tb_Boost.Text = "";
                }

                tb_Boost.Text = Run.RightCurrent.ToString();
                tb_EBrake.Text = Run.RightBaseline.ToString();
                tb_Reverse.Text = Run.Duck.ToString();
                //tb_A.Text = Run.Punch.ToString();
            }


            if (RPM.Text != Pedal.MovementSpan.TotalMilliseconds.ToString("0.000")) {
                using (StreamWriter w = File.AppendText(LogFile)) {
                    w.WriteLine(Pedal.MovementSteps.ToString() + "," + Pedal.MovementSpan.TotalMilliseconds.ToString("0.000"));
                    // Update the underlying file.
                    w.Flush();
                    w.Close();
                }
            }

            RPM.Text = Pedal.MovementSpan.TotalMilliseconds.ToString("0.000");
            tb_PedalCount.Text = Pedal.MovementSteps.ToString();

        }

        private float CalcTurn(float firstside, float otherside, float current) {
            if ((firstside - otherside) > 0.16f) {
                current = 100;
            } else if ((firstside - otherside) > 0.14f) {
                if (current <= 82.5) {
                    current = 100;
                }
            } else if ((firstside - otherside) > 0.12f) {
                if (current <= 75) {
                    current = 100;
                }
            } else if ((firstside - otherside) > 0.08f) {
                if (current <= 62.5) {
                    current = 100;
                }
            } else if ((firstside - otherside) > 0.05f) {
                if (current <= 50) {
                    current = 100;
                }
            } else if ((firstside - otherside) > 0.02f) {
                if (current <= 40) {
                    current = 100;
                }
            }
            return current;
        }


        private Polyline getBodySegment(Microsoft.Kinect.JointCollection joints, Brush brush, params JointType[] ids) {
            PointCollection points = new PointCollection(ids.Length);
            for (int i = 0; i < ids.Length; ++i) {
                points.Add(getDisplayPosition(joints[ids[i]]));
            }

            Polyline polyline = new Polyline();
            polyline.Points = points;
            polyline.Stroke = brush;
            polyline.StrokeThickness = 5;
            return polyline;
        }

        private Point getDisplayPosition(Joint joint) {
            int depthX, depthY;
            var p = Kinect.MapSkeletonPointToDepth(joint.Position, DepthImageFormat.Resolution640x480Fps30);
 
            depthX = p.X;
            depthY = p.Y;

            // map back to skeleton.Width & skeleton.Height
            return new Point((int)(skeletonCanvas.Width * p.X / 640.0), (int)(skeletonCanvas.Height * p.Y / 480));
        }

        private static Dictionary<JointType, Brush> jointColors = new Dictionary<JointType, Brush>() { 
            {JointType.HipCenter, new SolidColorBrush(Color.FromRgb(169, 176, 155))},
            {JointType.Spine, new SolidColorBrush(Color.FromRgb(169, 176, 155))},
            {JointType.ShoulderCenter, new SolidColorBrush(Color.FromRgb(168, 230, 29))},
            {JointType.Head, new SolidColorBrush(Color.FromRgb(200, 0,   0))},
            {JointType.ShoulderLeft, new SolidColorBrush(Color.FromRgb(79,  84,  33))},
            {JointType.ElbowLeft, new SolidColorBrush(Color.FromRgb(84,  33,  42))},
            {JointType.WristLeft, new SolidColorBrush(Color.FromRgb(255, 126, 0))},
            {JointType.HandLeft, new SolidColorBrush(Color.FromRgb(215,  86, 0))},
            {JointType.ShoulderRight, new SolidColorBrush(Color.FromRgb(33,  79,  84))},
            {JointType.ElbowRight, new SolidColorBrush(Color.FromRgb(33,  33,  84))},
            {JointType.WristRight, new SolidColorBrush(Color.FromRgb(77,  109, 243))},
            {JointType.HandRight, new SolidColorBrush(Color.FromRgb(37,   69, 243))},
            {JointType.HipLeft, new SolidColorBrush(Color.FromRgb(77,  109, 243))},
            {JointType.KneeLeft, new SolidColorBrush(Color.FromRgb(69,  33,  84))},
            {JointType.AnkleLeft, new SolidColorBrush(Color.FromRgb(229, 170, 122))},
            {JointType.FootLeft, new SolidColorBrush(Color.FromRgb(255, 126, 0))},
            {JointType.HipRight, new SolidColorBrush(Color.FromRgb(181, 165, 213))},
            {JointType.KneeRight, new SolidColorBrush(Color.FromRgb(71, 222,  76))},
            {JointType.AnkleRight, new SolidColorBrush(Color.FromRgb(245, 228, 156))},
            {JointType.FootRight, new SolidColorBrush(Color.FromRgb(77,  109, 243))}
        };
        #endregion Skeleton Processing

        #region Private State
        private KinectSensor _Kinect;
        #endregion Private State

        private void SetValues_Click(object sender, RoutedEventArgs e) {
            
            Pedal.slowpedal = int.Parse(tb_PedalLowVal.Text);
            Pedal.mediumpedal = int.Parse(tb_PedalMediumVal.Text);
            Pedal.highpedal = int.Parse(tb_PedalBoostVal.Text);
            Pedal.LeanDeadZone = float.Parse(tb_DeadZoneVal.Text);
            Pedal.SetKeys(tb_GasKeyVal.Text.ToUpper().Trim(), tb_BoostKeyVal.Text.ToUpper().Trim(), tb_LeftKeyVal.Text.ToUpper().Trim(), tb_RightKeyVal.Text.ToUpper().Trim(), tb_BreakButton.Text.ToUpper().Trim(), tb_ReverseButton.Text.ToUpper().Trim());
            Pedal.AllowBrake = cb_LeftBreak.IsChecked.Value;
            Pedal.AllowReverse = cb_RightReverse.IsChecked.Value;
            Pedal.AllowLean = cb_Lean.IsChecked.Value;
            Pedal.AllowWheelies = cb_Wheelie.IsChecked.Value;
            Pedal.HeadLean = cb_LeanHead.IsChecked.Value;

        }

        private void ShowSetValues_Click(object sender, RoutedEventArgs e) {


            isFront = cb_IsFront.IsChecked.Value;
            ConfigSettings.IsOpen = true;

            ConfigSettings.Placement = PlacementMode.Center;
            ConfigSettings.PlacementTarget = this.Parent as System.Windows.UIElement;
            ConfigSettings.HorizontalOffset= -50;
            ConfigSettings.VerticalOffset = -50;

        }

        private void START_Click(object sender, RoutedEventArgs e) {
            if (Pedal.Active) {
                START.Background = Brushes.LightGreen;
                Pedal.Active = false;
                Run.Active = false;
                START.Content = "ACTIVATE";
            } else {
                START.Background = Brushes.Red;
                Pedal.Active = true;
                Run.Active = true;
                START.Content = "STOP";
            }

            if (!isBiking) {
                Run.Baseline();
            }

        }

        private void Preset_Click(object sender, RoutedEventArgs e) {
            var button = (Button)sender;


            currentPreset.ClearValue(Button.BackgroundProperty);
            switch (button.Name) {
                case "Preset1":
                    Pedal.SetKeys("A", "LSHIFT", "LEFT","RIGHT", "SPACE","Z");
                    Pedal.slowpedal = 1200;
                    Pedal.mediumpedal = 800;
                    Pedal.highpedal = 650;
                    button.Background = Brushes.LightGreen;
                    break;
                case "Preset2":
                    Pedal.SetKeys("W", "C", "A", "D", "SPACE","S");
                    Pedal.slowpedal = 1200;
                    Pedal.mediumpedal = 800;
                    Pedal.highpedal = 650;
                    button.Background = Brushes.LightGreen;
                    break;
                case "Preset3":
                    Pedal.SetKeys("W", "LSHIFT", "A", "D", "SPACE","S");
                    Pedal.slowpedal = 1200;
                    Pedal.mediumpedal = 800;
                    Pedal.highpedal = 650;
                    button.Background = Brushes.LightGreen;
                    break;
                case "Preset4":
                    Pedal.SetKeys("A", "LSHIFT", "LEFT", "RIGHT", "SPACE","S");
                    Pedal.slowpedal = 1200;
                    Pedal.mediumpedal = 800;
                    Pedal.highpedal = 650;
                    button.Background = Brushes.LightGreen;
                    break;
                case "Preset5":
                    Pedal.SetKeys("A", "LSHIFT", "LEFT", "RIGHT", "SPACE","S");
                    Pedal.slowpedal = 1200;
                    Pedal.mediumpedal = 800;
                    Pedal.highpedal = 650;
                    button.Background = Brushes.LightGreen;
                    break;
                default:
                    break;
            }

            tb_PedalLowVal.Text = Pedal.slowpedal.ToString();
            tb_PedalMediumVal.Text = Pedal.mediumpedal.ToString();
            tb_PedalBoostVal.Text = Pedal.highpedal.ToString();

            tb_GasKeyVal.Text = Pedal.GasKey;
            tb_BoostKeyVal.Text = Pedal.BoostKey;
            
            currentPreset = button;
            FocusManager.SetFocusedElement(button, null);

        }

        private void CloseSettings_Click(object sender, RoutedEventArgs e) {
            SetValues_Click(sender, e);

            ConfigSettings.IsOpen = false;

            isBiking = cb_IsBiking.IsChecked.Value;

            if (cb_IsFront.IsChecked.Value != isFront) {
                if (isBiking) {
                    Pedal.SwapControls();
                } else {
                    Run.SwapControls();
                }
            }


        }
    }

}
